/*
 * Copyright (c) Huawei Technologies Co., Ltd. 2023-2024.
 * Description: RTOS self-developed code for phy_device.c
 * Author: Wang Haitao
 * Create: 2023/9/14
 */

#ifdef CONFIG_RTOS_NET_PHY_DOWNSHIFT
static unsigned int downshift_on = 1;

module_param(downshift_on, uint, 0444);
MODULE_PARM_DESC(downshift_on, "phy downshift support enable");

static int genphy_downshift_linkmode(struct phy_device *phydev)
{
	int adv, lpa, lpagb, common_adv, common_adv_gb = 0;

	if (phydev->is_gigabit_capable) {
		lpagb = phy_read(phydev, MII_STAT1000);
		if (lpagb < 0)
			return lpagb;

		adv = phy_read(phydev, MII_CTRL1000);
		if (adv < 0)
			return adv;

		common_adv_gb = lpagb & (adv << 2);
	}

	lpa = phy_read(phydev, MII_LPA);
	if (lpa < 0)
		return lpa;

	adv = phy_read(phydev, MII_ADVERTISE);
	if (adv < 0)
		return adv;

	common_adv = lpa & adv;

	phydev->speed = SPEED_10;
	phydev->duplex = DUPLEX_HALF;
	phydev->pause = 0;
	phydev->asym_pause = 0;

	if (common_adv_gb & (LPA_1000FULL | LPA_1000HALF)) {
		phydev->speed = SPEED_1000;

		if (common_adv_gb & LPA_1000FULL)
			phydev->duplex = DUPLEX_FULL;
	} else if (common_adv & (LPA_100FULL | LPA_100HALF)) {
		phydev->speed = SPEED_100;

		if (common_adv & LPA_100FULL)
			phydev->duplex = DUPLEX_FULL;
	} else {
		if (common_adv & LPA_10FULL)
			phydev->duplex = DUPLEX_FULL;
	}

	if (phydev->duplex == DUPLEX_FULL) {
		phydev->pause = lpa & LPA_PAUSE_CAP ? 1 : 0;
		phydev->asym_pause = lpa & LPA_PAUSE_ASYM ? 1 : 0;
	}

	return 0;
}

static bool net_phy_downshift(struct phy_device *phydev, int *err)
{
	if (downshift_on) {
		*err = genphy_downshift_linkmode(phydev);
		return true;
	}
	return false;
}
#else
static bool net_phy_downshift(struct phy_device *phydev, int *err)
{
	return false;
}
#endif